Contacts - 6D Loop contacts #1309

LudovicDeMatteis · 2024-09-24T14:27:28Z

This PR adds a new type of contact to Crocoddyl.
We defined loop contacts (i.e. between parts of the robot) with 6D constraints (constraint on the contact frames relative position and orientation)

This corrects the desired contact acceleration, by accouting for drift in position and velocity

for more information, see https://pre-commit.ci

nmansard · 2024-10-08T10:16:02Z

include/crocoddyl/multibody/contacts/multiple-contacts.hxx

@@ -399,14 +422,7 @@ void ContactModelMultipleTpl<Scalar>::updateRneaDiff(
    assert_pretty(it_m->first == it_d->first,
                  "it doesn't match the contact name between data and model");
    if (m_i->active) {
-      switch (m_i->contact->get_type()) {


@cmastalli Please have a look here following our discussion

nim65s

Can you provide a test for this, either in C++ or in python ?

bindings/python/crocoddyl/multibody/contacts/contact-6d-loop.cpp

nim65s · 2024-10-08T13:21:21Z

An entry in CHANGELOG.md would also be welcome

LudovicDeMatteis · 2024-10-08T15:39:38Z

Can you provide a test for this, either in C++ or in python ?

Adding new tests to the factory for these contacts is not direct for me, I will provide a python test for now.

LudovicDeMatteis · 2024-10-08T15:43:57Z

I added a Changelog entry, @nim65s let me know if that is enough

Kotochleb

calcDiff is still full of memory allocation, being a result of temporary matrices storing the results of multiplications. Those super long operations will have to be split into several smaller ones, but in a somewhat smart way to let compiler properly optimize them. For today, I don't have more time to investigate it and propose some changes, though I will come back to it tomorrow.

For now, it seems ok and seems to be working fine. For the reference, I tested it with Pinocchio v2.7.1.

include/crocoddyl/multibody/contacts/contact-6d-loop.hxx

include/crocoddyl/multibody/contacts/multiple-contacts.hxx

for more information, see https://pre-commit.ci

nim65s · 2024-10-10T08:04:03Z

ref. LudovicDeMatteis#2

Kotochleb

The code looks much better now. The only thing left are dynamic memory allocations

include/crocoddyl/multibody/contacts/contact-6d-loop.hpp

include/crocoddyl/multibody/contacts/contact-6d-loop.hxx

bindings/python/crocoddyl/multibody/contacts/contact-6d-loop.cpp

LudovicDeMatteis · 2024-10-11T22:32:52Z

@nim65s I added unitests in C++ to the PR. I decided to create a different factory for loop contacts as they are quite different from standard contacts.
I also cherrypicked a commit from Maxime Sabbah's PR to fix an error in updateForce.

LudovicDeMatteis · 2024-10-11T22:34:10Z

@Kotochleb most of the code should be fine now, I may have missed or overlook some things though. Let me know if some things should still be changed or if the PR is fine as it is.

for more information, see https://pre-commit.ci

Kotochleb

Aside from those very minor changes mentioned in this review, everything appears to be all right. I ran unittests for the code compiled in both Release with -O3 and Release with -O3 -march=native -mavx -mfma. Both cases are passing. The same goes for dynamic memory allocation and and matrix resizing checks from Eigen. If the suggestions will be resolved I will approve the PR

include/crocoddyl/multibody/contacts/contact-6d-loop.hxx

bindings/python/crocoddyl/multibody/contacts/contact-6d-loop.cpp

Remove bindings for old data and change operation order for dvel_dq

LudovicDeMatteis · 2024-10-16T08:33:52Z

Aside from those very minor changes mentioned in this review, everything appears to be all right. I ran unittests for the code compiled in both Release with -O3 and Release with -O3 -march=native -mavx -mfma. Both cases are passing. The same goes for dynamic memory allocation and and matrix resizing checks from Eigen. If the suggestions will be resolved I will approve the PR

I made these changes @Kotochleb. I compiled in Debug just fine

cmastalli

First of all, thanks for pushing this relevant feature, @LudovicDeMatteis!

This PR handles closed-loop kinematics based on an implicit method, which certainly looks like our acceleration-based scleronomic contact constraints. However, there are small, yet important differences, i.e.,

"Closed-loop kinematics (or a chord) produces constraint forces in two bodies"

Moreover, in a general connectivity graph, we have a cluster composed of more than one close-loop kinematics (or chords). Each chord can be defined by two bodies that are constrained, with forces that, of course, propagate in each of them.

In short, I propose extending these notions to our "contact abstract class". To simplify the changes, I describe the action task for the simple case of a cluster with a single chord (your work) below. But, perhaps, we should later make it better. Let's discuss this in a meeting.

Rename ContactModelAbstractTpl to KinematicConstraintModelAbstractTpl and ContactDataAbstractTpl to KinematicConstraintDataAbstractTpl. This involves a deprecation procedure for the ContactModelAbstractTpl and ContactDataAbstractTpl.
Rename ContactItemTpl to KinematicConstraintItemTpl. This includes the deprecation of ContactItemTpl.
Rename ContactModelMultipleTpl to KinematicConstraintModelMultipleTpl and ContactDataMultipleTpltoKinematicConstraintDataMultipleTpl. Again, it involves a deprecation procedure for ContactModelMultipleTplandContactDataMultipleTpl`
Update unit test given changes in 1, 2, 3, and 4. This includes renaming files.
Introduce a std::list for the two bodies' frame IDs and placements in https://github.com/loco-3d/crocoddyl/blob/devel/include/crocoddyl/multibody/contact-base.hpp#L174-L175. This involves adapting setter and getter functions. Note that it assumes a single chord per cluster.
Introduce the notion of a list of forces in ForceDataAbstractTpl. This will allow us to store fext for the two bodies. To do this, our KinematicConstraintModelAbstract needs to include the notion of "the number of constrained forces".
Introduce a std::list for a0 and da0_dx for each body in https://github.com/loco-3d/crocoddyl/blob/devel/include/crocoddyl/multibody/contact-base.hpp#L212-L213. Optionally, we could take the opportunity to rename a0 and da0_dx to ac and dac_dx, respectively.
Rename ContactModel6DLoopTpl to KinematicLoopModelTpl. Same for the data structure.
Simplify ContactModel6DLoopTpl and ContactData6DLoopTpl by reusing the objects in the KinematicConstraintModelAbstractTpl, ForceDataAbstractTpl, and KinematicConstraintDataAbstractTpl classes. This requires changes in the Python bindings.

Unfortunately, I am asking for a bunch of painful things to develop. I am sorry about this and am open to discussing possibilities. However, I must say that this type of work should be done together to avoid these types of "extreme" changes.

I thank you for your effort, @LudovicDeMatteis and @nmansard! I am really interested in having such a feature in Crocoddyl, as you guys do. But, I believe we need to do it better.

I'll be in humanoids, perhaps we can also further discussing this topic there

cmastalli · 2024-10-11T10:08:00Z

include/crocoddyl/multibody/contacts/contact-6d-loop.hpp

+  ContactModel6DLoopTpl(boost::shared_ptr<StateMultibody> state,
+                        const int joint1_id, const SE3 &joint1_placement,
+                        const int joint2_id, const SE3 &joint2_placement,
+                        const pinocchio::ReferenceFrame ref,


To be consistent with other contacts, let's rename this to type, e.g.,

Suggested change

const pinocchio::ReferenceFrame ref,

const pinocchio::ReferenceFrame type,

Please adapt the documentation, other constructors, and Python bindings appropriately.

Keeping this for the discussion for now

Following last week discussion, I changed ref to type to remain consistent with other contacts API. If we need some renaming, this should be done in another PR for refactoring

cmastalli · 2024-10-11T10:12:45Z

include/crocoddyl/multibody/contacts/contact-6d-loop.hxx

+  pinocchio::updateFramePlacements<Scalar>(*state_->get_pinocchio().get(),
+                                           *d->pinocchio);


This is very inefficient. We should update the frame placements for joints 1 and 2.

I removed this since we actually didn't need frame placements (but only joint placements)

cmastalli · 2024-10-11T10:18:44Z

include/crocoddyl/multibody/contacts/contact-6d-loop.hxx

+  d->j1Xf1.noalias() = joint1_placement_.toActionMatrix();
+  d->j2Xf2.noalias() = joint2_placement_.toActionMatrix();


We are not interested in storing these values, but their inverses. Moreover, their inverses are constant and should be computed when creating data. See this example: https://github.com/loco-3d/crocoddyl/blob/devel/include/crocoddyl/multibody/contact-base.hpp#L193

As a final note, we are not using id or fXj from the base class. We should pick the joint 1 to store these values, similar spirit to the development in the paper. This indeed leads to many other comments such as

We should use get/set_reference and get/set_id to store joint 1 information.

Indeed, I changed the way values are stored and computed. For the final note, I suggest that we wait for the discussion in case anything changes on that.

Adding a comment for the final note. As discussed, we are using parent joint id and relative placement to identify the contact points here. However, Base::id is a FrameIndex not a jointIndex, so it cannot be used directly. I suggest we wait for a larger refactoring to change this properly.

cmastalli · 2024-10-11T10:21:17Z

include/crocoddyl/multibody/contacts/contact-6d-loop.hxx

+        gains_[1] *
+        (joint1_placement_.toActionMatrixInverse() * d->v1_partial_dq -
+         d->f1Mf2.act(d->f2vf2).toActionMatrix() *
+             (d->f1Jf1 - d->f1Xf2 * d->f2Jf2) -


This is inefficient. We should use Jc.

This comment is on outdated code, it has been changed since

cmastalli · 2024-10-11T10:21:29Z

include/crocoddyl/multibody/contacts/contact-6d-loop.hxx

+         d->f1Xf2 * joint2_placement_.toActionMatrixInverse() *
+             d->v2_partial_dq);
+    d->da0_dx.rightCols(nv).noalias() +=
+        gains_[1] * (d->f1Jf1 - d->f1Xf2 * d->f2Jf2);


Same here, let's use Jc.

Same here, the code is outdated

…iently

for more information, see https://pre-commit.ci

LudovicDeMatteis force-pushed the topic/contact-6D-closed-loop branch from cbaace3 to 40327f6 Compare September 24, 2024 15:22

LudovicDeMatteis force-pushed the topic/contact-6D-closed-loop branch from 257f608 to 837ac62 Compare October 8, 2024 07:15

LudovicDeMatteis and others added 4 commits October 8, 2024 09:16

Add Contact 6D calc and calcDiff

ea346d3

Update forces for 6D loop contacts

5a6b507

Add Baumgarte corrector for contact 6D

c3d0fb5

This corrects the desired contact acceleration, by accouting for drift in position and velocity

[pre-commit.ci] auto fixes from pre-commit.com hooks

edebda5

for more information, see https://pre-commit.ci

LudovicDeMatteis force-pushed the topic/contact-6D-closed-loop branch from 837ac62 to edebda5 Compare October 8, 2024 07:16

nmansard reviewed Oct 8, 2024

View reviewed changes

nim65s reviewed Oct 8, 2024

View reviewed changes

bindings/python/crocoddyl/multibody/contacts/contact-6d-loop.cpp Outdated Show resolved Hide resolved

bindings/python/crocoddyl/multibody/contacts/contact-6d-loop.cpp Outdated Show resolved Hide resolved

LudovicDeMatteis added 2 commits October 8, 2024 17:33

Update copyrights to 2024

53524d2

Remove unwanted GCC diagnostic pop

081f828

Add Changelog entry

c5fdaf0

Kotochleb suggested changes Oct 9, 2024

View reviewed changes

LudovicDeMatteis and others added 3 commits October 9, 2024 18:03

Review changes - add noalias, change operation order and fix alocation

db0183e

[pre-commit.ci] auto fixes from pre-commit.com hooks

6b24284

for more information, see https://pre-commit.ci

Remove f_cross from bindings

30576fc

Fixed some malloc in calc

6ea5e9e

Kotochleb suggested changes Oct 11, 2024

View reviewed changes

LudovicDeMatteis and others added 4 commits October 11, 2024 20:22

Update docstring and fix malloc in calcDiff

b10e29e

Add unitests for contact loop 6D

3ed2d53

Cherrypick - Fix updateForceDiff for contact loop, add missing -

1e408b1

Fix malloc in updateForce

47f1672

LudovicDeMatteis force-pushed the topic/contact-6D-closed-loop branch from 62fe727 to 47f1672 Compare October 11, 2024 22:11

LudovicDeMatteis mentioned this pull request Oct 11, 2024

Merge changes in actuation, contact force residual and fix on force derivative for 6d contact loop LudovicDeMatteis/crocoddyl#2

Open

Fix docstrings and data variable init

29fbf6e

LudovicDeMatteis force-pushed the topic/contact-6D-closed-loop branch from 4617b2a to 29fbf6e Compare October 11, 2024 22:35

pre-commit-ci bot and others added 2 commits October 11, 2024 22:35

[pre-commit.ci] auto fixes from pre-commit.com hooks

5012fe0

for more information, see https://pre-commit.ci

Add dummy variables to store unused data from pinocchio functions

5b883ba

Kotochleb suggested changes Oct 15, 2024

View reviewed changes

include/crocoddyl/multibody/contacts/contact-6d-loop.hxx Outdated Show resolved Hide resolved

bindings/python/crocoddyl/multibody/contacts/contact-6d-loop.cpp Outdated Show resolved Hide resolved

bindings/python/crocoddyl/multibody/contacts/contact-6d-loop.cpp Outdated Show resolved Hide resolved

Apply review - Bindings and operation order

f9de7b3

Remove bindings for old data and change operation order for dvel_dq

Kotochleb approved these changes Oct 16, 2024

View reviewed changes

LudovicDeMatteis requested a review from nim65s October 16, 2024 09:13

cmastalli assigned cmastalli and unassigned cmastalli Nov 5, 2024

cmastalli self-requested a review November 5, 2024 10:17

cmastalli requested changes Nov 5, 2024

View reviewed changes

LudovicDeMatteis and others added 4 commits November 5, 2024 14:20

Review - change on jXf variables to be computed and stored more effic…

27a72e3

…iently

[pre-commit.ci] auto fixes from pre-commit.com hooks

ba26e58

for more information, see https://pre-commit.ci

Update ref to type in contact 6d loop

080a2f3

[pre-commit.ci] auto fixes from pre-commit.com hooks

ee66a5c

for more information, see https://pre-commit.ci

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Contacts - 6D Loop contacts #1309

Contacts - 6D Loop contacts #1309

LudovicDeMatteis commented Sep 24, 2024

nmansard Oct 8, 2024

nim65s left a comment

nim65s commented Oct 8, 2024

LudovicDeMatteis commented Oct 8, 2024

LudovicDeMatteis commented Oct 8, 2024

Kotochleb left a comment •

edited

Loading

nim65s commented Oct 10, 2024

Kotochleb left a comment

LudovicDeMatteis commented Oct 11, 2024

LudovicDeMatteis commented Oct 11, 2024

Kotochleb left a comment

LudovicDeMatteis commented Oct 16, 2024

cmastalli left a comment •

edited

Loading

cmastalli Oct 11, 2024

LudovicDeMatteis Nov 5, 2024

LudovicDeMatteis Nov 18, 2024

cmastalli Oct 11, 2024

LudovicDeMatteis Nov 5, 2024

cmastalli Oct 11, 2024

LudovicDeMatteis Nov 5, 2024

LudovicDeMatteis Nov 18, 2024

cmastalli Oct 11, 2024

LudovicDeMatteis Nov 5, 2024

cmastalli Oct 11, 2024

LudovicDeMatteis Nov 5, 2024

	const pinocchio::ReferenceFrame ref,
	const pinocchio::ReferenceFrame type,

		pinocchio::updateFramePlacements<Scalar>(*state_->get_pinocchio().get(),
		*d->pinocchio);

		d->j1Xf1.noalias() = joint1_placement_.toActionMatrix();
		d->j2Xf2.noalias() = joint2_placement_.toActionMatrix();

Contacts - 6D Loop contacts #1309

Are you sure you want to change the base?

Contacts - 6D Loop contacts #1309

Conversation

LudovicDeMatteis commented Sep 24, 2024

Choose a reason for hiding this comment

nim65s left a comment

Choose a reason for hiding this comment

nim65s commented Oct 8, 2024

LudovicDeMatteis commented Oct 8, 2024

LudovicDeMatteis commented Oct 8, 2024

Kotochleb left a comment • edited Loading

Choose a reason for hiding this comment

nim65s commented Oct 10, 2024

Kotochleb left a comment

Choose a reason for hiding this comment

LudovicDeMatteis commented Oct 11, 2024

LudovicDeMatteis commented Oct 11, 2024

Kotochleb left a comment

Choose a reason for hiding this comment

LudovicDeMatteis commented Oct 16, 2024

cmastalli left a comment • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Kotochleb left a comment •

edited

Loading

cmastalli left a comment •

edited

Loading